#ifndef _ROTATEEXPMAP_
#define _ROTATEEXPMAP_

#include "Transformation.h"
#include "Quaternion.h"
#include <cmath>
#include <cassert>
#include "moremath.h"


Mat3d MakeSkewSymmetric(const Vec3d &v);
Vec3d FromSkewSymmetric(const Mat3d& m);

class RotateExpMap: public Transformation{
	public:
		RotateExpMap(Dof *x, Dof *y, Dof *z);
		RotateExpMap(Dof *x, Dof *y, Dof *z, char *mName);
		void applyGLTransform();
		bool NeedDofAdjustment();
		bool AdjustDofs();
	protected:
		Mat3d getDerivativeMatNew(Dof *d);
		Mat4d *getTransformMat();
		Mat4d *getDerivativeMat(Dof *d);
		Mat4d *getDerivativeMat(Dof *d1, Dof *d2);
	private:
		// some math utility functions
		double get_dq_dv(int i, int j, double theta, Vec3d v, Vec3d vhat);
		double get_dq_dv_approx(int i, int j, double theta, Vec3d v);
		double get_dq_dv_dv(int i, int j, int k, double theta, Vec3d vhat);
		double get_dq_dv_dv_approx(int i, int j, int k, double theta, Vec3d v);
		inline double Tsinc(double theta){
			return 0.5-sqr(theta)/48;
		}
		inline bool isZero(double theta){
			if(fabs(theta)<EPSILON) return true;
			else return false;
		}
};

#endif
